using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuUtilities
{
    /// <summary>
    /// 3行3列矩阵。
    /// </summary>
    public struct Matrix3x3
    {
        public Vector3 X;
        public Vector3 Y;
        public Vector3 Z;


        /// <summary>
        /// 获取3x3标识矩阵。
        /// </summary>
        public static Matrix3x3 Identity
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                Matrix3x3 toReturn;
                toReturn.X = new Vector3(1, 0, 0);
                toReturn.Y = new Vector3(0, 1, 0);
                toReturn.Z = new Vector3(0, 0, 1);
                return toReturn;
            }
        }

        /// <summary>
        /// 将两个矩阵的分量相加。
        /// </summary>
        /// <param name="a">要添加的第一个矩阵。</param>
        /// <param name="b">要添加的第二个矩阵。</param>
        /// <param name="result">两个输入矩阵之和。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(in Matrix3x3 a, in Matrix3x3 b, out Matrix3x3 result)
        {
            result.X = a.X + b.X;
            result.Y = a.Y + b.Y;
            result.Z = a.Z + b.Z;
        }

        /// <summary>
        /// 按标量缩放矩阵的组件。
        /// </summary>
        /// <param name="Matrix">要缩放的矩阵。</param>
        /// <param name="scale">要应用于矩阵组件的比例。</param>
        /// <param name="result">缩放矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Scale(Matrix3x3 matrix, float scale, out Matrix3x3 result)
        {
            result.X = matrix.X * scale;
            result.Y = matrix.Y * scale;
            result.Z = matrix.Z * scale;
        }

        /// <summary>
        /// 从一个矩阵中减去另一个矩阵的分量。
        /// </summary>
        /// <param name="a">要减去的矩阵。</param>
        /// <param name="b">要从a中减去的矩阵。</param>
        /// <param name="result">两个输入矩阵的差异。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Subtract(in Matrix3x3 a, in Matrix3x3 b, out Matrix3x3 result)
        {
            result.X = a.X - b.X;
            result.Y = a.Y - b.Y;
            result.Z = a.Z - b.Z;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe static void Transpose(M* m, M* transposed)
        {
            // 奇怪的功能！为什么？
            // 1)缺少一些对实际SIMD加速转置有用的指令。
            // 2)组件式访问较多,难以获取SIMD类型生成好胜编解码器。

            float m12 = m->M12;
            float m13 = m->M13;
            float m23 = m->M23;
            transposed->M11 = m->M11;
            transposed->M12 = m->M21;
            transposed->M13 = m->M31;

            transposed->M21 = m12;
            transposed->M22 = m->M22;
            transposed->M23 = m->M32;

            transposed->M31 = m13;
            transposed->M32 = m23;
            transposed->M33 = m->M33;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void Transpose(Matrix3x3* m, Matrix3x3* transposed)
        {
            Transpose((M*)m, (M*)transposed);
        }

        /// <summary>                                                                                                
        /// 计算矩阵的转置矩阵。
        /// </summary>                                                                                               
        /// <param name="m">要转置的矩阵。</param>
        /// <param name="transsposed">转置矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Transpose(in Matrix3x3 m, out Matrix3x3 transposed)
        {
            var xy = m.X.Y;
            var xz = m.X.Z;
            var yz = m.Y.Z;
            transposed.X = new Vector3(m.X.X, m.Y.X, m.Z.X);
            transposed.Y = new Vector3(xy, m.Y.Y, m.Z.Y);
            transposed.Z = new Vector3(xz, yz, m.Z.Z);
        }

        /// <summary>
        /// 计算矩阵的行列式。
        /// </summary>
        /// <return>矩阵的行列式。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float Determinant()
        {
            return Vector3.Dot(X, Vector3.Cross(Y, Z));
        }

        /// <summary>
        /// 反转给定矩阵。
        /// </summary>
        /// <param name="m">要反转的矩阵。</param>
        /// <param name="Inverse">逆矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Invert(in Matrix3x3 m, out Matrix3x3 inverse)
        {
            // 目前在没有洗牌的情况下,交叉的实现远远不是最优的,即使这样,这也有一些改进的空间。
            // 倒置应该真的很少见,所以不用太担心。尽可能使用标量版本,直到ryujit改进(我们也改进了这个实现)。
            var yz = Vector3.Cross(m.Y, m.Z);
            var zx = Vector3.Cross(m.Z, m.X);
            var xy = Vector3.Cross(m.X, m.Y);
            var inverseDeterminant = 1f / Vector3.Dot(m.X, yz);
            inverse.X = yz * inverseDeterminant;
            inverse.Y = zx * inverseDeterminant;
            inverse.Z = xy * inverseDeterminant;
            Transpose(inverse, out inverse);
        }

        /// <summary>
        /// 反转给定矩阵。
        /// </summary>
        /// <param name="m">要反转的矩阵。</param>
        /// <param name="Inverse">逆矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void Invert(Matrix3x3* m, Matrix3x3* inverse)
        {
            var mScalar = (M*)m;
            var inverseScalar = (M*)inverse;

            var m11 = mScalar->M22 * mScalar->M33 - mScalar->M32 * mScalar->M23;
            var m21 = mScalar->M23 * mScalar->M31 - mScalar->M33 * mScalar->M21;
            var m31 = mScalar->M21 * mScalar->M32 - mScalar->M31 * mScalar->M22;
            var determinantInverse = 1f / (m11 * mScalar->M11 + m21 * mScalar->M12 + m31 * mScalar->M13);

            var m12 = mScalar->M32 * mScalar->M13 - mScalar->M12 * mScalar->M33;
            var m22 = mScalar->M33 * mScalar->M11 - mScalar->M13 * mScalar->M31;
            var m32 = mScalar->M31 * mScalar->M12 - mScalar->M11 * mScalar->M32;

            var m13 = mScalar->M12 * mScalar->M23 - mScalar->M22 * mScalar->M13;
            var m23 = mScalar->M13 * mScalar->M21 - mScalar->M23 * mScalar->M11;
            var m33 = mScalar->M11 * mScalar->M22 - mScalar->M21 * mScalar->M12;

            inverseScalar->M11 = m11 * determinantInverse;
            inverseScalar->M21 = m21 * determinantInverse;
            inverseScalar->M31 = m31 * determinantInverse;

            inverseScalar->M12 = m12 * determinantInverse;
            inverseScalar->M22 = m22 * determinantInverse;
            inverseScalar->M32 = m32 * determinantInverse;

            inverseScalar->M13 = m13 * determinantInverse;
            inverseScalar->M23 = m23 * determinantInverse;
            inverseScalar->M33 = m33 * determinantInverse;
        }






        /// <summary>
        /// 通过矩阵变换向量。
        /// </summary>
        /// <param name="v">要转换的向量3。</param>
        /// <param name="m">用作转换的矩阵。</param>
        /// <param name="result">转换的产品。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Transform(in Vector3 v, in Matrix3x3 m, out Vector3 result)
        {
            var x = new Vector3(v.X);
            var y = new Vector3(v.Y);
            var z = new Vector3(v.Z);
            result = m.X * x + m.Y * y + m.Z * z;
        }

        /// <summary>
        /// 通过矩阵的转置来变换向量。
        /// </summary>
        /// <param name="v">要转换的向量3。</param>
        /// <param name="m">用作转换转置的矩阵。</param>
        /// <param name="result">转换的产品。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformTranspose(in Vector3 v, in Matrix3x3 m, out Vector3 result)
        {
            result = new Vector3(
                Vector3.Dot(v, m.X),
                Vector3.Dot(v, m.Y),
                Vector3.Dot(v, m.Z));
        }

        struct M
        {
            public float M11, M12, M13;
            public float M21, M22, M23;
            public float M31, M32, M33;
        }


        /// <summary>
        /// 将两个矩阵相乘。
        /// </summary>
        /// <param name="a">要相乘的第一个矩阵。</param>
        /// <param name="b">要相乘的第二个矩阵。</param>
        /// <param name="result">乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Multiply(in Matrix3x3 a, in Matrix3x3 b, out Matrix3x3 result)
        {
            var bX = b.X;
            var bY = b.Y;
            {
                var x = new Vector3(a.X.X);
                var y = new Vector3(a.X.Y);
                var z = new Vector3(a.X.Z);
                result.X = x * bX + y * bY + z * b.Z;
            }

            {
                var x = new Vector3(a.Y.X);
                var y = new Vector3(a.Y.Y);
                var z = new Vector3(a.Y.Z);
                result.Y = x * bX + y * bY + z * b.Z;
            }

            {
                var x = new Vector3(a.Z.X);
                var y = new Vector3(a.Z.Y);
                var z = new Vector3(a.Z.Z);
                result.Z = x * bX + y * bY + z * b.Z;
            }
        }

        /// <summary>
        /// 将两个矩阵相乘,其中a被视为转置：result=transspose(A)*b
        /// </summary>
        /// <param name="a">要相乘的第一个将转置的矩阵。</param>
        /// <param name="b">要相乘的第二个矩阵。</param>
        /// <param name="result">乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void MultiplyTransposed(in Matrix3x3 a, in Matrix3x3 b, out Matrix3x3 result)
        {
            var bX = b.X;
            var bY = b.Y;
            {
                var x = new Vector3(a.X.X);
                var y = new Vector3(a.Y.X);
                var z = new Vector3(a.Z.X);
                result.X = x * bX + y * bY + z * b.Z;
            }

            {
                var x = new Vector3(a.X.Y);
                var y = new Vector3(a.Y.Y);
                var z = new Vector3(a.Z.Y);
                result.Y = x * bX + y * bY + z * b.Z;
            }

            {
                var x = new Vector3(a.X.Z);
                var y = new Vector3(a.Y.Z);
                var z = new Vector3(a.Z.Z);
                result.Z = x * bX + y * bY + z * b.Z;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromMatrix(in Matrix matrix, out Matrix3x3 matrix3x3)
        {
            matrix3x3.X = new Vector3(matrix.X.X, matrix.X.Y, matrix.X.Z);
            matrix3x3.Y = new Vector3(matrix.Y.X, matrix.Y.Y, matrix.Y.Z);
            matrix3x3.Z = new Vector3(matrix.Z.X, matrix.Z.Y, matrix.Z.Z);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromQuaternion(in Quaternion quaternion, out Matrix3x3 result)
        {
            float qX2 = quaternion.X + quaternion.X;
            float qY2 = quaternion.Y + quaternion.Y;
            float qZ2 = quaternion.Z + quaternion.Z;
            float XX = qX2 * quaternion.X;
            float YY = qY2 * quaternion.Y;
            float ZZ = qZ2 * quaternion.Z;
            float XY = qX2 * quaternion.Y;
            float XZ = qX2 * quaternion.Z;
            float XW = qX2 * quaternion.W;
            float YZ = qY2 * quaternion.Z;
            float YW = qY2 * quaternion.W;
            float ZW = qZ2 * quaternion.W;

            result.X = new Vector3(
                1 - YY - ZZ,
                XY + ZW,
                XZ - YW);

            result.Y = new Vector3(
                XY - ZW,
                1 - XX - ZZ,
                YZ + XW);

            result.Z = new Vector3(
                XZ + YW,
                YZ - XW,
                1 - XX - YY);


        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Matrix3x3 CreateFromQuaternion(in Quaternion quaternion)
        {
            CreateFromQuaternion(quaternion, out var toReturn);
            return toReturn;
        }


        /// <summary>
        /// 沿局部轴创建表示给定比例的3x3矩阵。
        /// </summary>
        /// <param name="scale">要表示的比例。</param>
        /// <param name="linearTransform">表示刻度的矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateScale(in Vector3 scale, out Matrix3x3 linearTransform)
        {
            linearTransform.X = new Vector3(scale.X, 0, 0);
            linearTransform.Y = new Vector3(0, scale.Y, 0);
            linearTransform.Z = new Vector3(0, 0, scale.Z);
        }

        /// <summary>
        /// 创建表示从轴和角度派生的旋转的矩阵。
        /// </summary>
        /// <param name="axis">旋转轴。</param>
        /// <param name="angle">旋转的角度。</param>
        /// <param name="result">结果旋转矩阵。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Matrix3x3 result)
        {
            // 待办事项：可以简化得更好。
            float xx = axis.X * axis.X;
            float yy = axis.Y * axis.Y;
            float zz = axis.Z * axis.Z;
            float xy = axis.X * axis.Y;
            float xz = axis.X * axis.Z;
            float yz = axis.Y * axis.Z;

            float sinAngle = (float)Math.Sin(angle);
            float oneMinusCosAngle = 1 - (float)Math.Cos(angle);

            result.X = new Vector3(
                1 + oneMinusCosAngle * (xx - 1),
                axis.Z * sinAngle + oneMinusCosAngle * xy,
                -axis.Y * sinAngle + oneMinusCosAngle * xz);

            result.Y = new Vector3(
                -axis.Z * sinAngle + oneMinusCosAngle * xy,
                1 + oneMinusCosAngle * (yy - 1),
                axis.X * sinAngle + oneMinusCosAngle * yz);

            result.Z = new Vector3(
                axis.Y * sinAngle + oneMinusCosAngle * xz,
                -axis.X * sinAngle + oneMinusCosAngle * yz,
                1 + oneMinusCosAngle * (zz - 1));

        }

        /// <summary>
        /// 创建表示从轴和角度派生的旋转的矩阵。
        /// </summary>
        /// <param name="axis">旋转轴。</param>
        /// <param name="angle">旋转的角度。</param>
        /// <return>结果旋转矩阵。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Matrix3x3 CreateFromAxisAngle(in Vector3 axis, float angle)
        {
            CreateFromAxisAngle(axis, angle, out var result);
            return result;

        }

        /// <summary>
        /// 创建一个矩阵,使得a x v=a*结果。
        /// </summary>
        /// <param name="v">要从中构建倾斜对称矩阵的向量。</param>
        /// <param name="result">表示叉积的对称矩阵不对称。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateCrossProduct(in Vector3 v, out Matrix3x3 result)
        {
            result.X.X = 0f;
            result.X.Y = -v.Z;
            result.X.Z = v.Y;
            result.Y.X = v.Z;
            result.Y.Y = 0f;
            result.Y.Z = -v.X;
            result.Z.X = -v.Y;
            result.Z.Y = v.X;
            result.Z.Z = 0f;
        }

        /// <summary>
        /// 连接两个矩阵。
        /// </summary>
        /// <param name="M1">第一个输入矩阵。</param>
        /// <param name="m2">第二个输入矩阵。</param>
        /// <return>形式为m1*m2的串联转换。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Matrix3x3 operator *(in Matrix3x3 m1, in Matrix3x3 m2)
        {
            Multiply(m1, m2, out var toReturn);
            return toReturn;
        }
    }
}
